﻿using System;
using System.Collections.Generic;
using System.Text;
using DecodeInterface;
using System.Drawing;
namespace MotionDetect
{
    public class ParticleFilter : DecodeBitmap
    {
        bool startTrack = false;
        bool initialTrack = false;
        Rectangle rectTrack = new Rectangle();
        Point[] pointFeatures = new Point[0];        //特征点
        byte[] mouldFeature = new byte[0];
        static readonly Size SizeAvailable = new Size(10, 10);      //运动框有效大小
        static readonly Size SizePadding = new Size(50, 50);        //内边框有效距离
        static readonly Rectangle RectParticle = new Rectangle(-25, -25, 50, 50);//粒子范围
        static int intersectNumber = 25;//与动态区域相交计数器
        static Color colorTrack = Color.Yellow;
        Random random = new Random();
        Size sizeDeltaSum = new Size(1, 1);
        Rectangle rectAvailable;

        //public ParticleFilter() { }
        //public ParticleFilter(Control decodeShow) : base(decodeShow) { }

        protected override unsafe object[] OnDecode(byte* pIn, byte* pOut, Size sizeIn, Size sizeOut, params object[] state)
        {
            if (state.Length < 4)
                throw new ApplicationException("third object and fourth object of state is not exsit|ParticleFilter");

            List<Rectangle> rectMotion = state[2] as List<Rectangle>;
            Rectangle rectMax = (Rectangle)state[3];

            Point[] particles = new Point[0];   //粒子坐标

            //新空图像
            for (int yOut = 0; yOut < sizeOut.Height; yOut++)
            {
                for (int xOut = 0; xOut < sizeOut.Width; xOut++)
                {
                    int index = DataManager.GetIndex(xOut, yOut, sizeOut.Width);
                    pOut[index] = pOut[index + 1] = pOut[index + 2] = 0;
                }
            }

            if (rectMax != Rectangle.Empty && !startTrack)
            {
                startTrack = true;
                rectTrack = rectMax;
            }

            if (startTrack)
            {
                if (!initialTrack)
                {//初始化
                    //提取特征点
                    rectAvailable = new Rectangle(new Point(), sizeOut);
                    rectAvailable.Inflate(-SizePadding.Width, -SizePadding.Width);

                    pointFeatures = DataManager.GetRandomPoint(
                        new Rectangle(new Point(), rectTrack.Size), 500);
                    //提取模板特征直方图
                    mouldFeature = DataManager.GetFeatureHistogram(rectTrack, pIn, sizeOut, pointFeatures);

                    initialTrack = true;

                    this.sizeDeltaSum = new Size(1, 1);
                }
                else
                {
                    //重采样，重新获取粒子坐标
                    Rectangle rectParticle = new Rectangle(
                        rectTrack.Left + RectParticle.X,
                        rectTrack.Top + RectParticle.Y,
                        RectParticle.Width, RectParticle.Height);
                    particles = DataManager.GetRandomPoint(rectParticle, 20);

                    Point pointMaxFeature = rectTrack.Location;
                    double maxFeature = 0;
                    byte[] particleMaxFeature = new byte[0];

                    //求最大相似度的粒子
                    for (int i = 0; i < particles.Length; i++)
                    {
                        //获取粒子特征
                        byte[] particleFeature = DataManager.GetFeatureHistogram(
                            new Rectangle(particles[i], rectTrack.Size), pIn, sizeOut, pointFeatures);
                        //获取粒子特征与模板特征的相似度
                        double feature = DataManager.GetSimilarFeature(mouldFeature, particleFeature);
                        if (feature > maxFeature)
                        {
                            maxFeature = feature;
                            pointMaxFeature = particles[i];
                            particleMaxFeature = particleFeature;
                        }
                    }
                    rectTrack.Location = pointMaxFeature;
                    //double[] similars = new double[particles.Length];
                    ////求各粒子处相似度
                    //double sumSimilars = 0;
                    //double avgFeature = 0;
                    //for (int i = 0; i < particles.Length; i++)
                    //{
                    //    //获取粒子特征直方图
                    //    byte[] particleFeature = DataManager.GetFeatureHistogram(
                    //        new Rectangle(particles[i], rectTrack.Size), pIn, sizeOut, pointFeatures);
                    //    //获取粒子特征与模板特征的相似度
                    //    similars[i] = DataManager.GetSimilarFeature(mouldFeature, particleFeature);
                    //    sumSimilars += similars[i];
                    //}
                    ////归一化权值后求粒子加权平均坐标
                    //Point pointAvgFeature = new Point();
                    //for (int i = 0; i < particles.Length; i++)
                    //{
                    //    pointAvgFeature.X += (int)(particles[i].X * similars[i] / sumSimilars);
                    //    pointAvgFeature.Y += (int)(particles[i].Y * similars[i] / sumSimilars);
                    //}
                    //particleMaxFeature = DataManager.GetFeatureHistogram(
                    //        new Rectangle(pointAvgFeature, rectTrack.Size), pIn, sizeOut, pointFeatures);
                    //avgFeature = DataManager.GetSimilarFeature(mouldFeature, particleMaxFeature);

                    //Point pointTrack = rectTrack.Location;//保存上一次跟踪区域
                    ////取最大相似度和加权平均处相似度大的点作为下一个跟踪区域的Location
                    //rectTrack.Location = maxFeature > avgFeature ? pointMaxFeature : pointAvgFeature;

                    //检测是否与运动区域相交，如果不相交则计数器减一
                    bool isIntersect = false;
                    foreach (var item in rectMotion)
                    {
                        if (item.IntersectsWith(rectTrack))
                        {
                            isIntersect = true;
                            break;
                        }
                    }
                    if (!isIntersect) intersectNumber--;

                    //跟踪目标丢失
                    if (!rectAvailable.Contains(rectTrack)
                        || maxFeature < 0.7
                        || intersectNumber <= 0)
                    {
                        initialTrack = false;
                        startTrack = false;
                        rectTrack = new Rectangle();
                        pointFeatures = new Point[0];
                        mouldFeature = new byte[0];
                        colorTrack = Color.FromArgb(
                            random.Next(256),
                            random.Next(256),
                            random.Next(256));
                        intersectNumber = 50;
                    }
                }
            }

            //绘图
            if (startTrack)
            {
                for (int h = rectTrack.Y - 20; h < rectTrack.Y + rectTrack.Height + 20; h++)
                {
                    for (int w = rectTrack.X - 20; w < rectTrack.X + rectTrack.Width + 20; w++)
                    {
                        int index = DataManager.GetIndex(w, h, sizeOut.Width);
                        pOut[index] = pOut[index + 1] = pOut[index + 2] = pIn[index];
                    }
                }

                //绘制跟踪框
                if (rectTrack.Width > 0 && rectTrack.Height > 0)
                    DataManager.DrawRectangle(rectTrack, colorTrack, pOut, sizeOut);

                //绘制粒子
                foreach (var p in particles)
                {
                    int index = DataManager.GetIndex(p.X, p.Y, sizeOut.Width);
                    pOut[index + 1] =
                    pOut[index + 2] = 255;
                }

                //绘制有效区域边框
                DataManager.DrawRectangle(rectAvailable, Color.Red, pOut, sizeOut);

                foreach (var rect in rectMotion)
                {
                    if (rect == rectMax)
                        DataManager.DrawRectangle(rectMax, Color.Red, pOut, sizeOut);
                    else
                        DataManager.DrawRectangle(rect, Color.Lime, pOut, sizeOut);
                }
            }

            if (this.startTrack)
                return new object[] { rectTrack };
            else
                return new object[] { Rectangle.Empty };
        }



    }
}
